// Slave

#define SCK_PIN   13
#define MISO_PIN  12
#define MOSI_PIN  11
#define SS_PIN    10

char buf [100];
volatile byte pos;

void SlaveInit(void) {
  // Set MISO output, all others input
  pinMode(SCK_PIN, INPUT);
  pinMode(MOSI_PIN, INPUT);
  pinMode(MISO_PIN, OUTPUT);
  pinMode(SS_PIN, INPUT);

  // turn on SPI in slave mode
  SPCR |= _BV(SPE);
  
  // turn on interrupts
  SPCR |= _BV(SPIE);
}

// SPI interrupt routine
ISR (SPI_STC_vect)
{
  byte c = SPDR;


  // SPI Slave Recieve test loop  
  if(c == 'g')
  {
    // test IF; until working cRIO SPI code is up, we need this to test recieving and sending
    Serial.println("SPI g recieved, send gyroscope data");
    WriteByte(0x00); // This makes everything work, do not remove

    WriteByte('F');
    Serial.println((signed int)sen_data.gyro_x);
    WriteByte('R');
    Serial.println((signed int)sen_data.gyro_y);
    WriteByte('C');
    Serial.println((signed int)sen_data.gyro_z);
  }
  
  else if(c == 'a')
  {
    Serial.println("SPI a recieved, send accelerometer data");
    WriteByte(0x0);

    WriteByte((byte)sen_data.accel_x);
    WriteByte((byte)sen_data.accel_y);
    WriteByte((byte)sen_data.accel_z);
  }
  
  else if(c == 'b')
  {
    Serial.println("SPI m recieved, send barometer data");
    WriteByte(0x0);

    WriteByte((byte)sen_data.baro_temp);
    WriteByte((byte)sen_data.baro_pres);
  }
  
  else if(c == 'm')
  {
    Serial.println("SPI m recieved, send magnometer data");
    WriteByte(0x0);

    WriteByte((byte)sen_data.magnetom_x);
    WriteByte((byte)sen_data.magnetom_y);
    WriteByte((byte)sen_data.magnetom_z);
    WriteByte((byte)sen_data.magnetom_heading);
  }
  
  else if(c == 'z')
  {
    Serial.println("SPI z recieved, send all");
    WriteByte(0x0);

    // Gyro Data
    WriteByte((byte)sen_data.gyro_x);
    WriteByte((byte)sen_data.gyro_y);
    WriteByte((byte)sen_data.gyro_z);
    
    // Accelerometer Data
    WriteByte((byte)sen_data.accel_x);
    WriteByte((byte)sen_data.accel_y);
    WriteByte((byte)sen_data.accel_z);
    
    // Magnetometer Data
    WriteByte((byte)sen_data.magnetom_x);
    WriteByte((byte)sen_data.magnetom_y);
    WriteByte((byte)sen_data.magnetom_z);
    WriteByte((byte)sen_data.magnetom_heading);
    
    // Barometer and Thermometer Data
    WriteByte((byte)sen_data.baro_temp);
    WriteByte((byte)sen_data.baro_pres);
    
  }
    
}

byte ReadByte(void) {
  while(!(SPSR & (1<<SPIF)));
  // wait until data recieved flag is cleared, not used because it does not work well with the timing
  return SPDR;
}

void WriteByte(byte value) {
  SPDR = value;
  while (!(SPSR & (1<<SPIF)));
  // wait until data sent flag is cleared
  return;
}
